/** ###################################################################
**     Filename  : mgm.c
**     Project   : mgm
**     Processor : MC9S12XDT512BCFU
**     Version   : Driver 01.14
**     Compiler  : CodeWarrior HCS12X C Compiler
**     Date/Time : 4/26/2012, 5:40 PM
**     Abstract  :
**         Main module.
**         This module contains user's application code.
**     Settings  :
**     Contents  :
**         No public methods
**
** ###################################################################*/
/* MODULE mgm */

/* Including needed modules to compile this module/procedure */
#include "Cpu.h"
#include "Events.h"
#include "AS1.h"
#include "GPIO1.h"
/* Include shared modules, which are used for whole project */
#include "PE_Types.h"
#include "PE_Error.h"
#include "PE_Const.h"
#include "IO_Map.h"

/* User includes (#include below this line is not maintained by Processor Expert) */
/* User includes (#include below this line is not maintained by Processor Expert) */
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <math.h>
#include <ctype.h>

#define mag_ID 5

typedef struct 
{
 word id;
 byte len;
 byte data[8];
} can_message_t;

typedef struct 
{
  float x;
  float y;
  float z;
} mag_vec_t;

byte can_recv( can_message_t *pmess);
byte can_send( can_message_t *pmess);
void can_init( void);

#define LED0 PORTA_PA7
#define LED1 PORTA_PA6

#define CANE   0x80
#define INITAK 0x01
#define INITRQ 0x01
#define SLPRQ  0x02
#define SLPAK  0x02

//char mag_buf[10] = "";
short mag_buf[10] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
byte mag_buf_index = 0;

mag_vec_t mag_vec;

void as1_send_str( const char *str)
{
  word len;
  byte i;
  
  len = strlen( str);
  for( i = 0; i < len; i++)
  {
    ( void) AS1_SendChar( str[i]);
    while( AS1_GetTxComplete() == 0) {};
  }
}

void main(void)
{
  /* Write your local variable definition here */
  int ret = 0; 
  can_message_t msg;
  byte c = 0;
  byte i = 0;
  short tmp;
  
  /*** Processor Expert internal initialization. DON'T REMOVE THIS CODE!!! ***/
  PE_low_level_init();
  /*** End of Processor Expert internal initialization.                    ***/

  //AS1_TurnRxOff();

  can_init();

  LED0 = 0;
  LED1 = 0;
  
  Cpu_Delay100US( 1000);

  /*// as1 baudrate 9600
  SCI0BD = 0x0104U;
  // set baudrate 19200
  as1_send_str( "*99WE\r");
  as1_send_str( "*99!BR=F\r");
  Cpu_Delay100US( 30);
  // as1 baudrate 19200
  SCI0BD = 0x0082U;*/

  // stop eventual continuous stream  
  //as1_send_str( "Esc\r");
  Cpu_Delay100US( 30);
  
  // set binary data format
  as1_send_str( "*99WE\r");
  Cpu_Delay100US(30);
  as1_send_str( "*99B\r");
  Cpu_Delay100US( 30);

  // set 50Hz sample rate 
  as1_send_str( "*99WE\r");
  Cpu_Delay100US( 30);
  as1_send_str( "*99R=100\r");
  Cpu_Delay100US( 30);

  // set continous ouptput mode
  //as1_send_str( "*99C\r"); // *99WE *99C
  //Cpu_Delay100US(30);
  
  //AS1_TurnRxOn();

  for( ;;)
  {
    ret = AS1_RecvChar( &c);

    if( ret == ERR_OK)
    {
      if( c == 0x0d && mag_buf_index == 6)
      {
        mag_buf_index = 0;
                
        LED0 = ~LED0;
        // x
        tmp = ( mag_buf[0] << 8) + mag_buf[1];
        mag_vec.x = ((float) tmp) / 15.0f; // convert to mG
        
        // y
        tmp = ( mag_buf[2] << 8) + mag_buf[3];
        mag_vec.y = ((float) tmp) / 15.0f; // convert to mG
        
        // z
        tmp = ( mag_buf[4] << 8) + mag_buf[5];
        mag_vec.z = ((float) tmp) / 15.0f; // convert to mG
        
        msg.id = 0x0518;
        msg.len = 8;
        msg.data[0] = mag_ID;
        msg.data[1] = 2;
        msg.data[2] = 0;
        msg.data[3] = 0;
        msg.data[4] = ( ( unsigned char*) &mag_vec.x)[0];
        msg.data[5] = ( ( unsigned char*) &mag_vec.x)[1];
        msg.data[6] = ( ( unsigned char*) &mag_vec.x)[2];
        msg.data[7] = ( ( unsigned char*) &mag_vec.x)[3];
        while( !can_send( &msg));

        msg.id = 0x0519;
        msg.len = 8;
        msg.data[0] = mag_ID;
        msg.data[1] = 2;
        msg.data[2] = 0;
        msg.data[3] = 0;
        msg.data[4] = ( ( unsigned char*) &mag_vec.y)[0];
        msg.data[5] = ( ( unsigned char*) &mag_vec.y)[1];
        msg.data[6] = ( ( unsigned char*) &mag_vec.y)[2];
        msg.data[7] = ( ( unsigned char*) &mag_vec.y)[3];
        while( !can_send( &msg));

        msg.id = 0x051a;
        msg.len = 8;
        msg.data[0] = mag_ID;
        msg.data[1] = 2;
        msg.data[2] = 0;
        msg.data[3] = 0;
        msg.data[4] = ( ( unsigned char*) &mag_vec.z)[0];
        msg.data[5] = ( ( unsigned char*) &mag_vec.z)[1];
        msg.data[6] = ( ( unsigned char*) &mag_vec.z)[2];
        msg.data[7] = ( ( unsigned char*) &mag_vec.z)[3];
        while( !can_send( &msg));
      }
      else
      {
        if( mag_buf_index < 6)
        {
          mag_buf[mag_buf_index] = c;
          mag_buf_index++;
        }
        else
        {
          mag_buf_index = 0;
        }
      }
    }

    // CAN request
    ret = can_recv( &msg);
    if( ret == 1)
    {  
      CAN0RFLG = 0x01;     
      if( msg.id == 1)
      {
        LED1 = ~LED1;
        as1_send_str( "*99P\r"); // sample pool
        mag_buf_index = 0;
      }
    }
  }

  /*** Processor Expert end of main routine. DON'T MODIFY THIS CODE!!! ***/
  for(;;){}
  /*** Processor Expert end of main routine. DON'T WRITE CODE BELOW!!! ***/
} /*** End of main routine. DO NOT MODIFY THIS TEXT!!! ***/

byte can_recv( can_message_t *pmess)
{
 byte i, tmp;
  
 if( CAN0RFLG & 0x01) 
 {
   pmess->id = CAN0RXIDR0 << 8;
   pmess->id += CAN0RXIDR1;
   if( pmess->id & 0x0010) 
   { 
     pmess->len = 0x80;
   }
   else
   {
     pmess->len = 0;
   }
  
   pmess->id = ( pmess->id >> 5) & 0x07ff;
                                                                                                    
   if( ( CAN0RXDLR & 0x0f) > 8) 
   {
     pmess->len |= 0x08;
     tmp = 8;
   }
   else	
   {
     tmp = CAN0RXDLR & 0x0f;
     pmess->len |= tmp;  
   }
   
   for( i=0; i < tmp; i++)
   {
     pmess->data[i] = (&CAN0RXDSR0)[i];
   }
   return 1;
  }
  return 0;
}

byte can_send( can_message_t *pmess)
{
  char i;
  unsigned short volatile *pireg;
 
  if( CAN0TFLG & 0x01)
  {
    pireg = ( unsigned short volatile *)( &CAN0TXIDR0);
    if( pmess->len & 0x80)
    { 
      *pireg = ( ( pmess->id << 5) & 0xffe0) | 0x0010;	// write id, rtr frame
    }
    else
    {
      *pireg = ( ( pmess->id << 5) & 0xffe0); // write id, data frame
    }
    pireg++;
    *pireg = 0;
    
    pmess->len &= 0x0f;
    for( i = 0; i < pmess->len; i++)
    {
      ( &CAN0TXDSR0)[i] = pmess->data[i];	  // write data
    }
    CAN0TXDLR = pmess->len; 	        // write dlc
    CAN0TFLG = 1;	 					  // start transmission
    return 1;
  }

  return 0;
}

void can_init(void)
{
  CAN0CTL0 |= INITRQ; // set initrq, this will also set initak
  while( !( CAN0CTL1 & INITAK)); // wait for init mode to occur

  CAN0CTL1 = CANE; // set cane just in case this is the first time after reset

  CAN0BTR0=0x01; // for 500kbps 0x03, for 100kbps 0x13
  CAN0BTR1=0x32; // for a can Baud of 500kbps at 16Mhz crystal

  // can id filter bits
  CAN0IDAR0=0x00;
  CAN0IDAR1=0x00;
  CAN0IDAR2=0x00;
  CAN0IDAR3=0x00;
  CAN0IDAR4=0x00;
  CAN0IDAR5=0x00;
  CAN0IDAR6=0x00;
  CAN0IDAR7=0x00;

  // can id filter mask
  CAN0IDMR0=0xff;
  CAN0IDMR1=0xff;
  CAN0IDMR2=0xff;
  CAN0IDMR3=0xff;
  CAN0IDMR4=0xff;
  CAN0IDMR5=0xff;
  CAN0IDMR6=0xff;
  CAN0IDMR7=0xff;

  CAN0IDAC = 0x10; // set filters to 2 16 bit acceptance filters

  CAN0CTL0 &= ~INITRQ; // clear initrq
  while (CAN0CTL1 & INITAK);

  CAN0CTL1 |= CANE; // set cane just in case this is the first time after reset
  CAN0RIER_RXFIE = 0; // enable receive interrupt

  CAN0TBSEL = 1;
}
/* END mgm */
/*
** ###################################################################
**
**     This file was created by Processor Expert 3.02 [04.44]
**     for the Freescale HCS12X series of microcontrollers.
**
** ###################################################################
*/
